import rclpy
import time
import json
import os
from rclpy.action import ActionServer
from rclpy.node import Node
from abb_interface.action import Abb
from abb_interface.msg import TaskRequest, TaskResponse
from rclpy.executors import MultiThreadedExecutor
from agv_ros2.scheduler2cmd import send_agv_task



class RobotNode(Node):
    def __init__(self):
        super().__init__('robot_node')

        # 创建robotActionServer，需要监听的主题是wsl_device_MOMA_ros2
        self.robot_actionServer = ActionServer(
            self,
            Abb,
            'wsl_device_MOMA_ros2',
            self.execute_callback   # 回调函数
        )
        # 读取配置文件中的站点映射（用于目标站点编号转换）
        self.station_mapping = self.load_station_mapping()

    def load_station_mapping(self):
        """加载agvparams.json中的站点映射关系"""
        json_path = "/mnt/d/ros_workspace/wsl_workspace/src/agv_ros2/agv_ros2/agvparams.json"
        try:
            with open(json_path, 'r', encoding='utf-8') as f:
                config = json.load(f)
            return config['station_mapping']
        except Exception as e:
            self.get_logger().error(f"加载站点配置失败: {e}")
            return {}

    def load_agv_params(self):
        """直接读取固定路径的agvparams.json"""
        json_path = "/mnt/d/ros_workspace/wsl_workspace/src/agv_ros2/agv_ros2/config/agvparams.json"
        try:
            with open(json_path, 'r', encoding='utf-8') as f:
                return json.load(f)
        except FileNotFoundError:
            self.get_logger().error(f"❌ 找不到agvparams.json！路径：{json_path}")
            return None
        except json.JSONDecodeError:
            self.get_logger().error(f"❌ agvparams.json格式错误！路径：{json_path}")
            return None

    def get_target_station_id(self, station_name):
        """从配置文件映射站点名称到编号"""
        params = self.load_agv_params()
        if not params or 'station_mapping' not in params:
            return None
        return params['station_mapping'].get(station_name, None)

    def doJob1(self, tmpJobid, tmpParams):
        target_name = tmpParams[0]
        target_id = self.get_target_station_id(target_name)
        if not target_id:
            self.get_logger().error(f"无法找到站点名称 {target_name} 对应的编号，请检查 agvparams.json")
            return False
        self.get_logger().info(f"MOMA 开始前往站点 {target_name}（目标编号：{target_id}）")

        # 1. 发送移动指令（此时返回的是JSON字符串）
        resp_str = send_agv_task(tmpJobid, tmpParams)
        print(resp_str)
        # 3. 闭环等待AGV到达（和之前逻辑一致，解析3003响应时也用同样方式）
        start_time = time.time()
        timeout = 60  # 超时时间60秒
        check_interval = 1  # 每秒检查一次

        while True:
            elapsed = time.time() - start_time
            if elapsed > timeout:
                self.get_logger().error(f"等待超时！AGV在 {timeout} 秒内未到达目标站点 {target_name}")
                return False

            # 4. 调用3003任务检查当前站点（返回JSON字符串）
            status_resp_str = send_agv_task("3003", ["station"])
            try:
                status_resp = json.loads(status_resp_str)
                print(status_resp_str)
            except json.JSONDecodeError:
                self.get_logger().warning(f"解析AGV状态响应失败: {status_resp_str}")
                time.sleep(check_interval)
                continue

            # 5. 判断状态查询是否成功
            if status_resp.get("status") != 200 or not status_resp.get("success", False):
                self.get_logger().warning(f"获取AGV站点状态失败: {status_resp.get('msg')}")
                time.sleep(check_interval)
                continue

            # 6. 提取当前站点并判断是否到达目标
            current_station = status_resp.get("response", {}).get("station", "")
            self.get_logger().info(f"当前站点: {current_station}，目标站点: {target_id}")

            if current_station == str(target_id):  # 确保和配置文件中的编号类型一致（字符串）
                self.get_logger().info(f"AGV已到达目标站点 {target_name}！")
                return True

            time.sleep(check_interval)

    def doJob2(self, tmpJobid, tmpParams):
        send_agv_task(tmpJobid, tmpParams)
        self.get_logger().info(f"MOMA 摆放瓶子 {tmpParams}")
        return True

    def doJob3(self, tmpJobid, tmpParams):
        send_agv_task(tmpJobid, tmpParams)
        self.get_logger().info(f"MOMA 状态查看 {tmpParams}")
        return True

    def doJob4(self, tmpJobid, tmpParams):
        send_agv_task(tmpJobid, tmpParams)
        self.get_logger().info(f"MOMA 充电 {tmpParams}")
        return True

    def execute_callback(self, goal_handle):
        tmpReq = goal_handle.request.task_request
        tmpTaskid = tmpReq.task_id
        tmpJobid = tmpReq.job_id
        tmpParams = tmpReq.params

        self.get_logger().info(f'机器人收到任务: {tmpTaskid}, {tmpJobid}, {tmpParams}')

        flag = False

        if tmpJobid == '3001':
            flag = self.doJob1(tmpJobid, tmpParams)
        elif tmpJobid == '3002':
            flag = self.doJob2(tmpJobid, tmpParams)
        elif tmpJobid == '3003':
            flag = self.doJob3(tmpJobid, tmpParams)
        elif tmpJobid == '3004':
            flag = self.doJob4(tmpJobid, tmpParams)
        else:
            self.get_logger().info(f'任务动作代码不存在: {tmpJobid}')

        goal_handle.succeed()
        result = Abb.Result()
        tmpresult = TaskResponse()
        tmpresult.task_id = tmpTaskid
        tmpresult.task_state = flag
        tmpresult.message = "job done!" if flag else "job failed or timeout"
        
        result.task_response = tmpresult
        return result

def main(args=None):
    rclpy.init(args=args)

    node = RobotNode()

    executor = MultiThreadedExecutor()
    executor.add_node(node)

    try:
        rclpy.spin(node, executor=executor)
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()